#include "newFuncs.hpp"

void recfunc(unsigned char buf)//receive from fly controller
{
	mavlink_message_t msg;
	mavlink_status_t status;
	if (mavlink_parse_char(0, buf, &msg, &status))
	
		if (msg.msgid == 0)
		{
			mavlink_heartbeat_t ht;
			mavlink_msg_heartbeat_decode(&msg, &ht);
			custom_mode = ht.custom_mode;
			base_mode = ht.base_mode;
		}
		if (msg.msgid == 253)
		{
			mavlink_statustext_t tp;
			mavlink_msg_statustext_decode(&msg, &tp);
			tp.text[49] = 0;
			mav_errmsg = string(tp.text);
		}
		else if (msg.msgid == 77)
		{
			mavlink_command_ack_t ta;
			mavlink_msg_command_ack_decode(&msg, &ta);
			//std::cout << ta.command << ":" << (int)ta.result << std::endl;
		}
		else if (msg.msgid == 65)
		{
			mavlink_rc_channels_t trc;
			mavlink_msg_rc_channels_decode(&msg, &trc);
			//std::cout << trc.chan1_raw << "  " << trc.chan2_raw << "  " << trc.chan3_raw << "  " << trc.chan4_raw << std::endl;
		}
		else if (msg.msgid == 30)
		{
			mavlink_attitude_t attitude;
			mavlink_msg_attitude_decode(&msg, &attitude);
			//	std::cout << "\b\b\b\b\b\b\b\b";
			//std::cout << attitude.pitch << ":" << attitude.roll << ":" << attitude.yaw;
			pitchmsg = attitude.pitch;
			rollmsg = attitude.roll;
			yawmsg = attitude.yaw;
		}
		else if (msg.msgid == 74)
		{
			mavlink_vfr_hud_t vfr_hud;
			mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
			deepmsg = vfr_hud.alt;									//remember to shift this back
			deepmsg = -0.1;
		}
		else if (msg.msgid == 1)
		{
			mavlink_sys_status_t sys_ss;
			mavlink_msg_sys_status_decode(&msg, &sys_ss);
			battery_v = sys_ss.voltage_battery;
		}
		else if (msg.msgid == 46)
		{
			std::cout << (int)msg.msgid << std::endl;
		}
	}
}

void pixbeatfunc(std::error_code err，asio::steady_timer* pixbeater,mavlink_channel_t chan)
{
	mavlink_msg_heartbeat_send(chan, 1, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
	pixbeater->expires_from_now(asio::chrono::milliseconds(700));
	pixbeater->async_wait(std::bind(&pixbeatfunc,std::placeholders::_1,pixbeater));
}

void check_stream(std::error_code err,asio::steady_timer* msg_io,mavlink_channel_t chan)
{
	if (battery_v == 0 && yawmsg == 0)
	{
		printmsg("no stream msg and try to request it again");
		mavlink_msg_request_data_stream_send(chan, 1, 190, 0, 10, 1);
		msg_io->expires_from_now(asio::chrono::seconds(1)); 
		msg_io->async_wait(std::bind(&check_stream,std::placeholders::_1,msg_io));
	}
	printmsg("OK");
}

bool movekeepfunc(std::error_code err,asio::steady_timer* movekeeptimer,mavlink_channel_t chan)
{
	if (!can_detect)
	{
		v_state.x = 0;
		v_state.y = 0;
		v_state.z = 500;
		v_state.r = 0;
	}
	mavlink_msg_manual_control_send(chan,1,v_state.x,v_state.y,v_state.z,v_state.r,0);
	movekeeptimer.expires_from_now(asio::chrono::milliseconds(500));
	movekeeptimer.async_wait(std::bind(&movekeepfunc,std::placeholders::_1,movekeeptimer,chan));
}

void move(int x, int y, int z, int r)
{
	v_state.x = x;
	v_state.y = y;
	v_state.z = z+500;
	v_state.r = r;
}

void change_mode(int modenum,mavlink_channel_t chan)
{
	mavlink_msg_set_mode_send(chan, 1, 1, modenum);
}